#include <imu_driver/imu_driver.h>

ImuDriver::ImuDriver()
    : rclcpp::Node("imu_driver_node")
{
  // 构造函数，用于初始化IMU驱动
  InitParameters();
  InitPublishers();
  InitFrameHandlers();
  InitSerial();
  ProcessLoop();
}

ImuDriver::~ImuDriver()
{
  // 析构函数，用于清理资源，例如关闭串口连接等
  if (__serial.isOpen())
    __serial.close();
}

/**
 * @brief 初始化IMU驱动参数
 *
 * 此函数初始化IMU驱动所需的参数，包括调试模式、设备类型、串口参数、话题名称和帧ID等。
 */
void ImuDriver::InitParameters()
{

  // 调试模式参数初始化
  this->declare_parameter<bool>("if_debug", false);
  __if_debug = this->get_parameter("if_debug").as_bool();

  // 设备类型参数初始化
  this->declare_parameter<int>("device_type", 1);
  __device_type = this->get_parameter("device_type").as_int();

  // 串口参数初始化
  this->declare_parameter<std::string>("serial_port", "/dev/ttyUSB0");
  __port = this->get_parameter("serial_port").as_string();

  this->declare_parameter<int>("baud_rate", 921600);
  __baudrate = this->get_parameter("baud_rate").as_int();

  this->declare_parameter<int>("serial_timeout", 2000);
  __serial_timeout = this->get_parameter("serial_timeout").as_int();

  // 话题名称参数初始化
  this->declare_parameter<std::string>("imu_topic", "/imu/data_raw");
  __imu_topic_name = this->get_parameter("imu_topic").as_string();

  // this->declare_parameter<std::string>("imu_pose_topic", "/imu_pose");
  // __imu_pose_topic_name = this->get_parameter("imu_pose_topic").as_string();

  // 帧ID参数初始化
  this->declare_parameter<std::string>("imu_frame_id", "imu_link");
  __imu_frame_id = this->get_parameter("imu_frame_id").as_string();

  // this->declare_parameter<std::string>("imu_pose_frame_id", "imu_link");
  // __imu_pose_frame_id = this->get_parameter("imu_pose_frame_id").as_string();
}

/**
 * @brief 初始化IMU数据的发布者
 *
 * 该函数初始化IMU数据的发布者，包括IMU数据的发布者和IMU姿态数据的发布者。
 */
void ImuDriver::InitPublishers()
{
  __imu_pub = this->create_publisher<sensor_msgs::msg::Imu>(__imu_topic_name.c_str(), 10);
  // __imu_pos_pub = this->create_publisher<geometry_msgs::msg::PoseWithCovariance>(__imu_pose_topic_name.c_str(), 10);
}

void ImuDriver::InitSerial()
{

  try
  {
    __serial.setPort(__port);
    __serial.setBaudrate(__baudrate);
    serial::Timeout _timeout = serial::Timeout::simpleTimeout(__serial_timeout); // 设置超时时间
    __serial.setTimeout(_timeout);
    __serial.open();

    if (__serial.isOpen())
    {
      RCLCPP_INFO(this->get_logger(), "Serial opened.");
    }
    else
    {
      RCLCPP_ERROR(this->get_logger(), "Serial open failed.");
      exit(0); // 退出程序
    }
  }
  catch (const std::exception &e)
  {
    RCLCPP_ERROR(this->get_logger(), "Exception: %s", e.what());
    exit(0); // 退出程序
  }
}

void ImuDriver::InitFrameHandlers()
{
  __frame_handlers[TYPE_IMU] = [this](const std::vector<uint8_t>& payload)
  {
    std::memcpy(&__imu_frame.frame.data.data_pack, payload.data(), payload.size());
    HandleImuFrame();
  };

  __frame_handlers[TYPE_AHRS] = [this](const std::vector<uint8_t>& payload)
  {
    std::memcpy(&__ahrs_frame.frame.data.data_pack, payload.data(), payload.size());
    HandleAhrsFrame();
  };

  __frame_handlers[TYPE_INSGPS] = [this](const std::vector<uint8_t>& payload)
  {
    (void)payload;
    HandleInsGpsFrame();
  };

  __frame_handlers[TYPE_GEODETIC_POS] = [this](const std::vector<uint8_t>& payload)
  {
    (void)payload;
    HandleGeoFrame();
  };
}

void ImuDriver::ProcessLoop()
{
  RCLCPP_INFO(this->get_logger(), "ImuDriver::ProcessLoop: start");

  while (rclcpp::ok())
  {
    if (!__serial.isOpen())
    {
      RCLCPP_WARN(this->get_logger(), "serial unopen");
    }
   
    FDILink::fdilink_header header;

    // 找到起始帧，读取帧头，并校验
    if (!ReadFrameHeader(header))
    {
      continue;
    }

    std::vector<uint8_t> payload;
    // 读取帧数据区 并校验
    if (!ReadFramePayload(header, payload))
    {
      continue;
    }

    // 处理已知数据帧，根据类型调用相应的处理函数
    auto it = __frame_handlers.find(header.data_type);
    if (it != __frame_handlers.end())
    {
      // RCLCPP_INFO(this->get_logger(), "Handling frame type 0x%02X", header.data_type);
      it->second(payload);
    }
    else
    {
      RCLCPP_WARN(this->get_logger(), "Unknown frame type 0x%02X", header.data_type);
    }
  }
}

/**
 * @brief 从串口读取帧头信息
 *
 * 从串口读取IMU数据帧的起始帧头以及后续帧头信息，并将其填充到传入的header结构体中。
 *
 * @param header 用于存储读取到的帧头信息的结构体
 * @return 如果成功读取并填充了帧头信息，则返回true；否则返回false
 */
bool ImuDriver::ReadFrameHeader(FDILink::fdilink_header &header)
{
  // 1. 读取起始帧头字节，循环读取直到读到 FRAME_HEAD 起始标志 0xFC
  uint8_t byte;
 
  if (__serial.read(&byte, 1) != 1)
  {
    RCLCPP_ERROR(this->get_logger(), "Timeout waiting for frame start");
    return false;
  }
  if (byte != FRAME_HEAD)
  {
    return false;
  }

  header.header_start = byte; // 起始标志

  // 2. 读取类型字节，校验数据类型
  if (__serial.read(&byte, 1) != 1)
  {
    RCLCPP_ERROR(this->get_logger(), "Timeout waiting for frame type");
    return false;
  }
  header.data_type = byte; // 数据类型

  if (header.data_type != TYPE_IMU &&
      header.data_type != TYPE_AHRS &&
      header.data_type != TYPE_INSGPS &&
      header.data_type != TYPE_GEODETIC_POS &&
      header.data_type != 0x50 &&
      header.data_type != TYPE_GROUND &&
      header.data_type != 0xff)
  {
    RCLCPP_WARN(this->get_logger(), "head_type error: %02X ", header.data_type);
    return false;
  }

  // 3. 读取长度字节，校验数据长度
  if (__serial.read(&byte, 1) != 1)
  {
    RCLCPP_ERROR(this->get_logger(), "Timeout waiting for frame length");
    return false;
  }
  header.data_size = byte;
  // 校验数据长度是否符合预期
  if (!ValidateDataLength(header))
  {
    RCLCPP_WARN(this->get_logger(), "Invalid data length for type 0x%02X", header.data_type);
    return false;
  }

  // 4. 处理特殊或未知数据帧，比如 TYPE_GROUND 或 0x50，例如地面数据帧
  if (header.data_type == TYPE_GROUND || header.data_type == 0x50)
  {
    ProcessSpecialFrame(header);
    return false;
  }

  // 5. 读取剩余帧头4字节
  uint8_t header_buf[4];
  if (__serial.read(header_buf, sizeof(header_buf)) != sizeof(header_buf))
  {
    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Incomplete frame header");
    return false;
  }

  // 4. 解析剩余帧头字段
  // header.data_size = header_buf[0];
  header.serial_num = header_buf[0];
  header.header_crc8 = header_buf[1];
  header.header_crc16_h = header_buf[2];
  header.header_crc16_l = header_buf[3];

  // CRC8校验
  if (!ValidateHeaderCRC8(header))
    return false;

  // 序列号处理
  ProcessSerialNumber(header);

  // 4. 调试打印函数
  if (__if_debug)
  {
    PrintDebugHeader(header);
  }
  return true;
}

/**
 * @brief 处理特殊帧
 *
 * 处理从传感器接收到的特殊帧，包括序列号的校验和更新，以及读取剩余的数据区。
 *
 * @param header FDILink帧头信息
 */
void ImuDriver::ProcessSpecialFrame(const FDILink::fdilink_header &header)
{
  uint8_t byte;
  // 读取序列号字节
  uint8_t ground_sn = header.serial_num;
  
  if (__serial.read(&byte, 1) == 1)
  {
    ground_sn = byte;
  }
 
  uint8_t expected_data_len = header.data_size; // 帧头里已经有数据长度

  // 序列号校验与更新
  if (++__read_sn != ground_sn)
  {
    if (__if_debug)
    {
      RCLCPP_WARN(this->get_logger(), "Detected SN lost. Expected %u but got %u", __read_sn, ground_sn);
    }

    if (ground_sn < __read_sn)
    {
      __sn_lost += 256 - (int)(__read_sn - ground_sn);
    }
    else
    {
      __sn_lost += (int)(ground_sn - __read_sn);
    }

    __read_sn = ground_sn;
  }

  // 读取剩余数据区(data_len + 4字节，比如CRC和帧尾)
  const size_t remaining_len = expected_data_len + 4;
  if (__serial.available() < remaining_len)
  {
    if (__if_debug)
    {
      RCLCPP_WARN(this->get_logger(), "Not enough bytes to read ground data. Expected %zu bytes", remaining_len);
    }
    return;
  }

  uint8_t ground_ignore[512] = {0};
  size_t read_ignore_bytes = __serial.read(ground_ignore, remaining_len);

  if (read_ignore_bytes != remaining_len && __if_debug)
  {
    RCLCPP_WARN(this->get_logger(), "Failed to read complete ground data. Expected %zu got %zu", remaining_len, read_ignore_bytes);
  }
}


/**
 * @brief 验证数据长度是否合法
 *
 * 根据传入的 FDILink 头信息验证数据长度是否合法。
 *
 * @param header FDILink 头信息
 * @return 若数据长度合法返回 true，否则返回 false
 */
bool ImuDriver::ValidateDataLength(const FDILink::fdilink_header &header)
{
  switch (header.data_type)
  {
  case TYPE_IMU:
    if (header.data_size != IMU_LEN)
    {
      RCLCPP_WARN(this->get_logger(), "head_len error (IMU)");
      return false;
    }
    break;
  case TYPE_AHRS:
    if (header.data_size != AHRS_LEN)
    {
      RCLCPP_WARN(this->get_logger(), "head_len error (AHRS)");
      return false;
    }
    break;
  case TYPE_INSGPS:
    if (header.data_size != INSGPS_LEN)
    {
      RCLCPP_WARN(this->get_logger(), "head_len error (INSGPS)");
      return false;
    }
    break;
  case TYPE_GEODETIC_POS:
    if (header.data_size != GEODETIC_POS_LEN)
    {
      RCLCPP_WARN(this->get_logger(), "head_len error (GEODETIC_POS)");
      return false;
    }
    break;
  default:
    // RCLCPP_WARN(this->get_logger(), "Unknown data_type: 0x%02X", header.data_type);
    return true;
  }
  return true;
}

/**
 * @brief 验证FDILink头部CRC8校验和
 *
 * 该函数验证传入的FDILink头部结构体的CRC8校验和是否匹配。
 *
 * @param header FDILink头部结构体
 * @return 如果CRC8校验和匹配，返回true；否则返回false
 */
bool ImuDriver::ValidateHeaderCRC8(const FDILink::fdilink_header &header)
{
  uint8_t crc8_calc = CRC8_Table(reinterpret_cast<const uint8_t *>(&header), 4);
  if (crc8_calc != header.header_crc8)
  {
    RCLCPP_WARN(this->get_logger(), "Header CRC8 mismatch: calc=0x%02X, recv=0x%02X", crc8_calc, header.header_crc8);
    return false;
  }
  return true;
}

void ImuDriver::ProcessSerialNumber(const FDILink::fdilink_header &header)
{

  static bool _first_sn = true; // 用于初始化，只在第一次运行时有效

  if (_first_sn)
  {
    // 第一次收到帧，初始化基准序列号
    __read_sn = header.serial_num - 1;
    _first_sn = false;
    return; // 第一帧不做丢包判断
  }

  // 先自增预期序列号
  if (++__read_sn != header.serial_num)
  {
    int diff = 0;
    if (header.serial_num < __read_sn)
    {
      diff = 256 - (int)(__read_sn - header.serial_num);
    }
    else
    {
      diff = (int)(header.serial_num - __read_sn);
    }
    __sn_lost += diff;

    if (__if_debug)
    {
      RCLCPP_WARN(this->get_logger(), "Detected SN lost on type 0x%02X, lost count +%d, total lost %d",
                  header.data_type, diff, __sn_lost);
    }
  }
  // 更新当前序列号
  __read_sn = header.serial_num;
}

// 通用读取数据函数，返回读取的数据区
bool ImuDriver::ReadFramePayload(const FDILink::fdilink_header &header, std::vector<uint8_t> &payload)
{
  const size_t data_len = header.data_size;
  const size_t total_len = data_len + 1; // data + frame_end
  int avail = __serial.available();
  if (avail < 0 || static_cast<size_t>(avail) < total_len)
  {
    RCLCPP_WARN(this->get_logger(), "Not enough bytes to read full frame");
    return false;
  }

  std::vector<uint8_t> data_buf(total_len);
  size_t read_len = __serial.read(data_buf.data(), total_len);
  if (read_len != total_len)
  {
    RCLCPP_WARN(this->get_logger(), "Failed to read full frame. Expected %zu, got %zu", total_len, read_len);
    return false;
  }

  // 校验 CRC16
  uint16_t crc_calc = CRC16_Table(data_buf.data(), data_len);
  uint16_t crc_recv = (static_cast<uint16_t>(header.header_crc16_h) << 8) | header.header_crc16_l;
  if (crc_calc != crc_recv)
  {
    RCLCPP_WARN(this->get_logger(), "CRC16 mismatch. Calc=0x%04X, Recv=0x%04X", crc_calc, crc_recv);
    return false;
  }

  // 校验帧尾
  if (data_buf.back() != FRAME_END)
  {
    RCLCPP_WARN(this->get_logger(), "Frame end mismatch. Expected 0x%02X, got 0x%02X", FRAME_END, data_buf.back());
    return false;
  }

  payload.resize(data_len);
  std::memcpy(payload.data(), data_buf.data(), data_len);
  return true;
}

void ImuDriver::HandleImuFrame()
{
  sensor_msgs::msg::Imu imu_msg;
  imu_msg.header.stamp = this->get_clock()->now();
  imu_msg.header.frame_id = __imu_frame_id.c_str();

  const auto &p = __imu_frame.frame.data.data_pack; // 直接用成员变量

  if (__device_type == 0)
  {
    // 未经变换的原始数据
    imu_msg.orientation.w = __ahrs_frame.frame.data.data_pack.Qw;
    imu_msg.orientation.x = __ahrs_frame.frame.data.data_pack.Qx;
    imu_msg.orientation.y = __ahrs_frame.frame.data.data_pack.Qy;
    imu_msg.orientation.z = __ahrs_frame.frame.data.data_pack.Qz;
    imu_msg.angular_velocity.x = p.gyroscope_x;
    imu_msg.angular_velocity.y = p.gyroscope_y;
    imu_msg.angular_velocity.z = p.gyroscope_z;
    imu_msg.linear_acceleration.x = p.accelerometer_x;
    imu_msg.linear_acceleration.y = p.accelerometer_y;
    imu_msg.linear_acceleration.z = p.accelerometer_z;
  }
  else if (__device_type == 1)
  {
    // 经过变换的数据:imu单品rclcpp标准下的坐标变换
    Eigen::Quaterniond q_ahrs(__ahrs_frame.frame.data.data_pack.Qw,
                              __ahrs_frame.frame.data.data_pack.Qx,
                              __ahrs_frame.frame.data.data_pack.Qy,
                              __ahrs_frame.frame.data.data_pack.Qz);
    Eigen::Quaterniond q_r =
        Eigen::AngleAxisd(PI, Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(PI, Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(0.00000, Eigen::Vector3d::UnitX());
    Eigen::Quaterniond q_rr =
        Eigen::AngleAxisd(0.00000, Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(0.00000, Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(PI, Eigen::Vector3d::UnitX());
    // Eigen::Quaterniond q_xiao_rr =
    //     Eigen::AngleAxisd(PI / 2, Eigen::Vector3d::UnitZ()) *
    //     Eigen::AngleAxisd(0.00000, Eigen::Vector3d::UnitY()) *
    //     Eigen::AngleAxisd(PI, Eigen::Vector3d::UnitX());

    Eigen::Quaterniond q_out = q_r * q_ahrs * q_rr;
    imu_msg.orientation.w = q_out.w();
    imu_msg.orientation.x = q_out.x();
    imu_msg.orientation.y = q_out.y();
    imu_msg.orientation.z = q_out.z();

    imu_msg.angular_velocity.x = p.gyroscope_x;
    imu_msg.angular_velocity.y = -p.gyroscope_y;
    imu_msg.angular_velocity.z = -p.gyroscope_z;
    imu_msg.linear_acceleration.x = p.accelerometer_x;
    imu_msg.linear_acceleration.y = -p.accelerometer_y;
    imu_msg.linear_acceleration.z = -p.accelerometer_z;
  }

  // geometry_msgs::msg::PoseWithCovariance pose_cov_msg;
  // // 1. orientation 拷贝
  // pose_cov_msg.pose.orientation = imu_msg.orientation;

  // // 2. position 通常IMU不提供位置 → 设为0或其他来源
  // pose_cov_msg.pose.position.x = 0.0;
  // pose_cov_msg.pose.position.y = 0.0;
  // pose_cov_msg.pose.position.z = 0.0;

  // // 3. covariance 设置
  // pose_cov_msg.covariance = robot_covariance_config::imu_pose_covariance;

  
  __imu_pub->publish(imu_msg);
  // __imu_pos_pub->publish(pose_cov_msg);
 
}

void ImuDriver::HandleAhrsFrame(){}

void ImuDriver::HandleInsGpsFrame(){}

void ImuDriver::HandleGeoFrame(){}

void ImuDriver::PrintDebugHeader(const FDILink::fdilink_header &h)
{

  // 打印帧头信息，包括起始字节、数据类型、大小、序列号和CRC校验码
  std::cout << " " << std::endl;
  std::cout << "check_head: " << std::hex << (int)h.header_start << std::dec << std::endl;
  std::cout << "head_type:  " << std::hex << (int)h.data_type << std::dec << std::endl;
  std::cout << "data_size:  " << std::dec << (int)h.data_size << std::endl;
  std::cout << "check_sn: " << std::hex << (int)h.serial_num << std::dec << std::endl;
  std::cout << "head_crc8: " << std::hex << (int)h.header_crc8 << std::dec << std::endl;
  std::cout << "head_crc16_H: " << std::hex << (int)h.header_crc16_h << std::dec << std::endl;
  std::cout << "head_crc16_L: " << std::hex << (int)h.header_crc16_l << std::dec << std::endl;
}